#require 'grouhbot.rb'
#$r = GrouhBot.new

class Motion
	attr_accessor :positions,:delay,:duree,:servos		
end 

def rbtest( dlyS = 0.001, dly1 = 0.150, dly2 = dly1, servos_plage = 0..15 )
	#$r.direct true
	sleep 0.300
	while true do
		chaine = ""
		servos_plage.each do|i|
			chaine += $r.wckReadPosDeg(i).to_s + ", "
			sleep dlyS
		end
		$r.direct false
		sleep dly1
		puts(chaine + " Accel:" + $r.accelerometer.join(','))
		$r.direct true
		sleep dly2
	end
end

def player( mv, torq = 1, dly = mv.delay, factor = 1.0 )
	x = 0
	#$r.direct true
	#sleep 0.3
	#toutmou(mv.servos)
	#sleep 0.3
	puts "init servos positions:"
	mv.positions[0].each_index do|servo|
		puts "#{servo}: #{mv.positions[0][servo]}°"
		x = $r.wckMovePosDeg( servo, mv.positions[0][servo], torq )
		sleep (1.0/15.0)
	end
	sleep 0.300
	mv.positions.each do|mouv|
		mouv.each_index do|servo|
			x = $r.wckMovePosDeg( servo, mouv[servo], torq )
			sleep(dly * factor)
		end
	end
end

def recorder( dly = 0.150, duree = 10, servos_plage = 0..15 )
	mvmt = Motion.new
	mvmt.delay = dly
	mvmt.duree = duree
	servos_plage = servos_plage.to_a
	mvmt.servos = servos_plage
	toutmou servos_plage	
	sleep 0.300
	puts "Start recording #{servos_plage.count} servos: #{servos_plage.join(', ')}"
	servos = []
	depart = Time.now
	tps_ecoule = 0.0
	while tps_ecoule < duree do
		servo = Array.new(servos_plage.count)
		servos_plage.each do|sId|
			servo[sId] = $r.wckReadPosDeg(sId)
			sleep dly
		end
		servos.push( servo )
		tps_ecoule = Time.now - depart
	end
	mvmt.positions = servos
	return mvmt
end

def toutmou( servos_plage = 0..15, dly = 0.0 )
	servos_plage.each do|sId|
		$r.wckPassive sId
		sleep dly if dly > 0.0
	end
end

def dcBasic
	begin
		$r.direct(false)
	rescue Exception => e
		puts e
	end
	sleep 1
	#$r.basic
	$r.direct(true) if ($r.basic && sleep(1))
	sleep 0.3
end

def saveAs( x, file_path, compressed = true )
	if not compressed
		File.open(file_path, 'w'){|f|
			f.write("0")
			x.positions.each{|pos|
				f.write(pos.join(',') + "\n");
			}
		}
	else			
		correspondances = []
		vals = x.positions.flatten.uniq
		vals.each_index do |index|
			correspondances[vals[index]] = index
		end
		vals.collect!{|v| correspondances[v] }
		File.open(file_path, 'w'){|f|
			f.write("1")
			f.write(correspondances.count.to_s + '.')
			correspondances.each_index do |idx|
				f.write(idx.to_s + correspondances[idx].to_s)
			end
			vals.each do |valeur|
				f.write valeur
			end
		}
	end
end

@direct = false
def direct 
	$r.direct( @direct )
	@direct = !@direct
end

def balance(seuil = 1 ,dly = 0.01, factor = 2, dlyMv = 0.6)
	$r.servoID_readservo 0
	old_pos = $r.getPos
	justBlanced = false
	while true 
		$r.servoID_readservo 0
		dR = ($r.getPos()[R_HIP] - old_pos[R_HIP]).abs
		dL = ($r.getPos()[L_HIP] - old_pos[L_HIP]).abs
		if not justBlanced
			if dR.abs >= seuil 
				$r.wckMovePos R_HIP, $r.getPos()[R_HIP] + dR * factor, 1
				justBlanced = true
			elsif dL.abs >= seuil
				$r.wckMovePos L_HIP, $r.getPos()[L_HIP] - dL * factor, 1
				justBlanced = true
			end
			sleep dly
		else
			sleep dlyMv
			justBlanced = false
		end
		old_pos = $r.getPos
	end
end


















